﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace CommonLib {
    public class DoubleTester {
        public static double Epsilon = 10e-10;
        public static void Assert(double a, double b) {
            double tmp = a - b;
            System.Diagnostics.Debug.Assert(IsZero(tmp));
        }
        public static bool IsZero(double d) {
            return -Epsilon <= d && d <= Epsilon;
        }
    }

    public class Vector3 {
        private double[] data;

        public Vector3() {
            initialize(0.0, 0.0, 0.0);
        }
        public Vector3(double x, double y, double z) {
            initialize(x, y, z);
        }
        public Vector3(Vector3 o) {
            initialize(o.data[0], o.data[1], o.data[2]);
        }
        public Vector3(double[] v) {
            initialize(v[0], v[1], v[2]);
        }
        private void initialize(double x, double y, double z) {
            this.data = new double[3];
            set(x, y, z);
        }

        public int Length {
            get { return this.data.Length; }
        }
        public Vector3 set(double d) {
            set(d, d, d);
            return this;
        }
        public Vector3 set(double x, double y, double z) {
            this.data[0] = x;
            this.data[1] = y;
            this.data[2] = z;
            return this;
        }
        public Vector3 set(Vector3 v) {
            set(v.data[0], v.data[1], v.data[2]);
            return this;
        }
        public double this[int r] {
            set {
                this.data[r] = value;
            }
            get {
                return this.data[r];
            }
        }
        public Vector3 add(Vector3 v) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] += v.data[i];
            }
            return this;
        }
        public Vector3 add(double b) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] += b;
            }
            return this;
        }
        public Vector3 sub(Vector3 v) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] -= v.data[i];
            }
            return this;
        }
        public Vector3 sub(double b) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] -= b;
            }
            return this;
        }
        public Vector3 mul(Vector3 v) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] *= v[i];
            }
            return this;
        }
        public Vector3 mul(double a) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] *= a;
            }
            return this;
        }
        public Vector3 mac(double a, Vector3 v) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] += a * v[i];
            }
            return this;
        }
        public Vector3 mac(Vector3 u, Vector3 v) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] += u[i] * v[i];
            }
            return this;
        }
        public double dot(Vector3 v) {
            double res = 0;
            for (int i = 0; i < this.data.Length; i++) {
                res += this.data[i] * v.data[i];
            }
            return res;
        }
        public Vector3 div(double denom) {
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] /= denom;
            }
            return this;
        }
        public Vector3 normalize() {
            double dist = norm();
            for (int i = 0; i < this.data.Length; i++) {
                this.data[i] /= dist;
            }
            return this;
        }
        public double norm() {
            return Math.Sqrt(dot(this));
        }
    }
}
